extern "C" {
#include <math.h>
/******************************************************************************
 *                       Code generated with sympy 1.4                        *
 *                                                                            *
 *              See http://www.sympy.org/ for more information.               *
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_6788465772816668761) {
   out_6788465772816668761[0] = delta_x[0] + nom_x[0];
   out_6788465772816668761[1] = delta_x[1] + nom_x[1];
   out_6788465772816668761[2] = delta_x[2] + nom_x[2];
   out_6788465772816668761[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3];
   out_6788465772816668761[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4];
   out_6788465772816668761[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5];
   out_6788465772816668761[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6];
   out_6788465772816668761[7] = delta_x[6] + nom_x[7];
   out_6788465772816668761[8] = delta_x[7] + nom_x[8];
   out_6788465772816668761[9] = delta_x[8] + nom_x[9];
   out_6788465772816668761[10] = delta_x[9] + nom_x[10];
   out_6788465772816668761[11] = delta_x[10] + nom_x[11];
   out_6788465772816668761[12] = delta_x[11] + nom_x[12];
   out_6788465772816668761[13] = delta_x[12] + nom_x[13];
   out_6788465772816668761[14] = delta_x[13] + nom_x[14];
   out_6788465772816668761[15] = delta_x[14] + nom_x[15];
   out_6788465772816668761[16] = delta_x[15] + nom_x[16];
   out_6788465772816668761[17] = delta_x[16] + nom_x[17];
   out_6788465772816668761[18] = delta_x[17] + nom_x[18];
   out_6788465772816668761[19] = delta_x[18] + nom_x[19];
   out_6788465772816668761[20] = delta_x[19] + nom_x[20];
   out_6788465772816668761[21] = delta_x[20] + nom_x[21];
   out_6788465772816668761[22] = delta_x[21] + nom_x[22];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_5601436874753092679) {
   out_5601436874753092679[0] = -nom_x[0] + true_x[0];
   out_5601436874753092679[1] = -nom_x[1] + true_x[1];
   out_5601436874753092679[2] = -nom_x[2] + true_x[2];
   out_5601436874753092679[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5];
   out_5601436874753092679[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4];
   out_5601436874753092679[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3];
   out_5601436874753092679[6] = -nom_x[7] + true_x[7];
   out_5601436874753092679[7] = -nom_x[8] + true_x[8];
   out_5601436874753092679[8] = -nom_x[9] + true_x[9];
   out_5601436874753092679[9] = -nom_x[10] + true_x[10];
   out_5601436874753092679[10] = -nom_x[11] + true_x[11];
   out_5601436874753092679[11] = -nom_x[12] + true_x[12];
   out_5601436874753092679[12] = -nom_x[13] + true_x[13];
   out_5601436874753092679[13] = -nom_x[14] + true_x[14];
   out_5601436874753092679[14] = -nom_x[15] + true_x[15];
   out_5601436874753092679[15] = -nom_x[16] + true_x[16];
   out_5601436874753092679[16] = -nom_x[17] + true_x[17];
   out_5601436874753092679[17] = -nom_x[18] + true_x[18];
   out_5601436874753092679[18] = -nom_x[19] + true_x[19];
   out_5601436874753092679[19] = -nom_x[20] + true_x[20];
   out_5601436874753092679[20] = -nom_x[21] + true_x[21];
   out_5601436874753092679[21] = -nom_x[22] + true_x[22];
}
void H_mod_fun(double *state, double *out_3068454842243606904) {
   out_3068454842243606904[0] = 1.0;
   out_3068454842243606904[1] = 0.0;
   out_3068454842243606904[2] = 0.0;
   out_3068454842243606904[3] = 0.0;
   out_3068454842243606904[4] = 0.0;
   out_3068454842243606904[5] = 0.0;
   out_3068454842243606904[6] = 0.0;
   out_3068454842243606904[7] = 0.0;
   out_3068454842243606904[8] = 0.0;
   out_3068454842243606904[9] = 0.0;
   out_3068454842243606904[10] = 0.0;
   out_3068454842243606904[11] = 0.0;
   out_3068454842243606904[12] = 0.0;
   out_3068454842243606904[13] = 0.0;
   out_3068454842243606904[14] = 0.0;
   out_3068454842243606904[15] = 0.0;
   out_3068454842243606904[16] = 0.0;
   out_3068454842243606904[17] = 0.0;
   out_3068454842243606904[18] = 0.0;
   out_3068454842243606904[19] = 0.0;
   out_3068454842243606904[20] = 0.0;
   out_3068454842243606904[21] = 0.0;
   out_3068454842243606904[22] = 0.0;
   out_3068454842243606904[23] = 1.0;
   out_3068454842243606904[24] = 0.0;
   out_3068454842243606904[25] = 0.0;
   out_3068454842243606904[26] = 0.0;
   out_3068454842243606904[27] = 0.0;
   out_3068454842243606904[28] = 0.0;
   out_3068454842243606904[29] = 0.0;
   out_3068454842243606904[30] = 0.0;
   out_3068454842243606904[31] = 0.0;
   out_3068454842243606904[32] = 0.0;
   out_3068454842243606904[33] = 0.0;
   out_3068454842243606904[34] = 0.0;
   out_3068454842243606904[35] = 0.0;
   out_3068454842243606904[36] = 0.0;
   out_3068454842243606904[37] = 0.0;
   out_3068454842243606904[38] = 0.0;
   out_3068454842243606904[39] = 0.0;
   out_3068454842243606904[40] = 0.0;
   out_3068454842243606904[41] = 0.0;
   out_3068454842243606904[42] = 0.0;
   out_3068454842243606904[43] = 0.0;
   out_3068454842243606904[44] = 0.0;
   out_3068454842243606904[45] = 0.0;
   out_3068454842243606904[46] = 1.0;
   out_3068454842243606904[47] = 0.0;
   out_3068454842243606904[48] = 0.0;
   out_3068454842243606904[49] = 0.0;
   out_3068454842243606904[50] = 0.0;
   out_3068454842243606904[51] = 0.0;
   out_3068454842243606904[52] = 0.0;
   out_3068454842243606904[53] = 0.0;
   out_3068454842243606904[54] = 0.0;
   out_3068454842243606904[55] = 0.0;
   out_3068454842243606904[56] = 0.0;
   out_3068454842243606904[57] = 0.0;
   out_3068454842243606904[58] = 0.0;
   out_3068454842243606904[59] = 0.0;
   out_3068454842243606904[60] = 0.0;
   out_3068454842243606904[61] = 0.0;
   out_3068454842243606904[62] = 0.0;
   out_3068454842243606904[63] = 0.0;
   out_3068454842243606904[64] = 0.0;
   out_3068454842243606904[65] = 0.0;
   out_3068454842243606904[66] = 0.0;
   out_3068454842243606904[67] = 0.0;
   out_3068454842243606904[68] = 0.0;
   out_3068454842243606904[69] = -0.5*state[4];
   out_3068454842243606904[70] = -0.5*state[5];
   out_3068454842243606904[71] = -0.5*state[6];
   out_3068454842243606904[72] = 0.0;
   out_3068454842243606904[73] = 0.0;
   out_3068454842243606904[74] = 0.0;
   out_3068454842243606904[75] = 0.0;
   out_3068454842243606904[76] = 0.0;
   out_3068454842243606904[77] = 0.0;
   out_3068454842243606904[78] = 0.0;
   out_3068454842243606904[79] = 0.0;
   out_3068454842243606904[80] = 0.0;
   out_3068454842243606904[81] = 0.0;
   out_3068454842243606904[82] = 0.0;
   out_3068454842243606904[83] = 0.0;
   out_3068454842243606904[84] = 0.0;
   out_3068454842243606904[85] = 0.0;
   out_3068454842243606904[86] = 0.0;
   out_3068454842243606904[87] = 0.0;
   out_3068454842243606904[88] = 0.0;
   out_3068454842243606904[89] = 0.0;
   out_3068454842243606904[90] = 0.0;
   out_3068454842243606904[91] = 0.5*state[3];
   out_3068454842243606904[92] = 0.5*state[6];
   out_3068454842243606904[93] = -0.5*state[5];
   out_3068454842243606904[94] = 0.0;
   out_3068454842243606904[95] = 0.0;
   out_3068454842243606904[96] = 0.0;
   out_3068454842243606904[97] = 0.0;
   out_3068454842243606904[98] = 0.0;
   out_3068454842243606904[99] = 0.0;
   out_3068454842243606904[100] = 0.0;
   out_3068454842243606904[101] = 0.0;
   out_3068454842243606904[102] = 0.0;
   out_3068454842243606904[103] = 0.0;
   out_3068454842243606904[104] = 0.0;
   out_3068454842243606904[105] = 0.0;
   out_3068454842243606904[106] = 0.0;
   out_3068454842243606904[107] = 0.0;
   out_3068454842243606904[108] = 0.0;
   out_3068454842243606904[109] = 0.0;
   out_3068454842243606904[110] = 0.0;
   out_3068454842243606904[111] = 0.0;
   out_3068454842243606904[112] = 0.0;
   out_3068454842243606904[113] = -0.5*state[6];
   out_3068454842243606904[114] = 0.5*state[3];
   out_3068454842243606904[115] = 0.5*state[4];
   out_3068454842243606904[116] = 0.0;
   out_3068454842243606904[117] = 0.0;
   out_3068454842243606904[118] = 0.0;
   out_3068454842243606904[119] = 0.0;
   out_3068454842243606904[120] = 0.0;
   out_3068454842243606904[121] = 0.0;
   out_3068454842243606904[122] = 0.0;
   out_3068454842243606904[123] = 0.0;
   out_3068454842243606904[124] = 0.0;
   out_3068454842243606904[125] = 0.0;
   out_3068454842243606904[126] = 0.0;
   out_3068454842243606904[127] = 0.0;
   out_3068454842243606904[128] = 0.0;
   out_3068454842243606904[129] = 0.0;
   out_3068454842243606904[130] = 0.0;
   out_3068454842243606904[131] = 0.0;
   out_3068454842243606904[132] = 0.0;
   out_3068454842243606904[133] = 0.0;
   out_3068454842243606904[134] = 0.0;
   out_3068454842243606904[135] = 0.5*state[5];
   out_3068454842243606904[136] = -0.5*state[4];
   out_3068454842243606904[137] = 0.5*state[3];
   out_3068454842243606904[138] = 0.0;
   out_3068454842243606904[139] = 0.0;
   out_3068454842243606904[140] = 0.0;
   out_3068454842243606904[141] = 0.0;
   out_3068454842243606904[142] = 0.0;
   out_3068454842243606904[143] = 0.0;
   out_3068454842243606904[144] = 0.0;
   out_3068454842243606904[145] = 0.0;
   out_3068454842243606904[146] = 0.0;
   out_3068454842243606904[147] = 0.0;
   out_3068454842243606904[148] = 0.0;
   out_3068454842243606904[149] = 0.0;
   out_3068454842243606904[150] = 0.0;
   out_3068454842243606904[151] = 0.0;
   out_3068454842243606904[152] = 0.0;
   out_3068454842243606904[153] = 0.0;
   out_3068454842243606904[154] = 0.0;
   out_3068454842243606904[155] = 0.0;
   out_3068454842243606904[156] = 0.0;
   out_3068454842243606904[157] = 0.0;
   out_3068454842243606904[158] = 0.0;
   out_3068454842243606904[159] = 0.0;
   out_3068454842243606904[160] = 1.0;
   out_3068454842243606904[161] = 0.0;
   out_3068454842243606904[162] = 0.0;
   out_3068454842243606904[163] = 0.0;
   out_3068454842243606904[164] = 0.0;
   out_3068454842243606904[165] = 0.0;
   out_3068454842243606904[166] = 0.0;
   out_3068454842243606904[167] = 0.0;
   out_3068454842243606904[168] = 0.0;
   out_3068454842243606904[169] = 0.0;
   out_3068454842243606904[170] = 0.0;
   out_3068454842243606904[171] = 0.0;
   out_3068454842243606904[172] = 0.0;
   out_3068454842243606904[173] = 0.0;
   out_3068454842243606904[174] = 0.0;
   out_3068454842243606904[175] = 0.0;
   out_3068454842243606904[176] = 0.0;
   out_3068454842243606904[177] = 0.0;
   out_3068454842243606904[178] = 0.0;
   out_3068454842243606904[179] = 0.0;
   out_3068454842243606904[180] = 0.0;
   out_3068454842243606904[181] = 0.0;
   out_3068454842243606904[182] = 0.0;
   out_3068454842243606904[183] = 1.0;
   out_3068454842243606904[184] = 0.0;
   out_3068454842243606904[185] = 0.0;
   out_3068454842243606904[186] = 0.0;
   out_3068454842243606904[187] = 0.0;
   out_3068454842243606904[188] = 0.0;
   out_3068454842243606904[189] = 0.0;
   out_3068454842243606904[190] = 0.0;
   out_3068454842243606904[191] = 0.0;
   out_3068454842243606904[192] = 0.0;
   out_3068454842243606904[193] = 0.0;
   out_3068454842243606904[194] = 0.0;
   out_3068454842243606904[195] = 0.0;
   out_3068454842243606904[196] = 0.0;
   out_3068454842243606904[197] = 0.0;
   out_3068454842243606904[198] = 0.0;
   out_3068454842243606904[199] = 0.0;
   out_3068454842243606904[200] = 0.0;
   out_3068454842243606904[201] = 0.0;
   out_3068454842243606904[202] = 0.0;
   out_3068454842243606904[203] = 0.0;
   out_3068454842243606904[204] = 0.0;
   out_3068454842243606904[205] = 0.0;
   out_3068454842243606904[206] = 1.0;
   out_3068454842243606904[207] = 0.0;
   out_3068454842243606904[208] = 0.0;
   out_3068454842243606904[209] = 0.0;
   out_3068454842243606904[210] = 0.0;
   out_3068454842243606904[211] = 0.0;
   out_3068454842243606904[212] = 0.0;
   out_3068454842243606904[213] = 0.0;
   out_3068454842243606904[214] = 0.0;
   out_3068454842243606904[215] = 0.0;
   out_3068454842243606904[216] = 0.0;
   out_3068454842243606904[217] = 0.0;
   out_3068454842243606904[218] = 0.0;
   out_3068454842243606904[219] = 0.0;
   out_3068454842243606904[220] = 0.0;
   out_3068454842243606904[221] = 0.0;
   out_3068454842243606904[222] = 0.0;
   out_3068454842243606904[223] = 0.0;
   out_3068454842243606904[224] = 0.0;
   out_3068454842243606904[225] = 0.0;
   out_3068454842243606904[226] = 0.0;
   out_3068454842243606904[227] = 0.0;
   out_3068454842243606904[228] = 0.0;
   out_3068454842243606904[229] = 1.0;
   out_3068454842243606904[230] = 0.0;
   out_3068454842243606904[231] = 0.0;
   out_3068454842243606904[232] = 0.0;
   out_3068454842243606904[233] = 0.0;
   out_3068454842243606904[234] = 0.0;
   out_3068454842243606904[235] = 0.0;
   out_3068454842243606904[236] = 0.0;
   out_3068454842243606904[237] = 0.0;
   out_3068454842243606904[238] = 0.0;
   out_3068454842243606904[239] = 0.0;
   out_3068454842243606904[240] = 0.0;
   out_3068454842243606904[241] = 0.0;
   out_3068454842243606904[242] = 0.0;
   out_3068454842243606904[243] = 0.0;
   out_3068454842243606904[244] = 0.0;
   out_3068454842243606904[245] = 0.0;
   out_3068454842243606904[246] = 0.0;
   out_3068454842243606904[247] = 0.0;
   out_3068454842243606904[248] = 0.0;
   out_3068454842243606904[249] = 0.0;
   out_3068454842243606904[250] = 0.0;
   out_3068454842243606904[251] = 0.0;
   out_3068454842243606904[252] = 1.0;
   out_3068454842243606904[253] = 0.0;
   out_3068454842243606904[254] = 0.0;
   out_3068454842243606904[255] = 0.0;
   out_3068454842243606904[256] = 0.0;
   out_3068454842243606904[257] = 0.0;
   out_3068454842243606904[258] = 0.0;
   out_3068454842243606904[259] = 0.0;
   out_3068454842243606904[260] = 0.0;
   out_3068454842243606904[261] = 0.0;
   out_3068454842243606904[262] = 0.0;
   out_3068454842243606904[263] = 0.0;
   out_3068454842243606904[264] = 0.0;
   out_3068454842243606904[265] = 0.0;
   out_3068454842243606904[266] = 0.0;
   out_3068454842243606904[267] = 0.0;
   out_3068454842243606904[268] = 0.0;
   out_3068454842243606904[269] = 0.0;
   out_3068454842243606904[270] = 0.0;
   out_3068454842243606904[271] = 0.0;
   out_3068454842243606904[272] = 0.0;
   out_3068454842243606904[273] = 0.0;
   out_3068454842243606904[274] = 0.0;
   out_3068454842243606904[275] = 1.0;
   out_3068454842243606904[276] = 0.0;
   out_3068454842243606904[277] = 0.0;
   out_3068454842243606904[278] = 0.0;
   out_3068454842243606904[279] = 0.0;
   out_3068454842243606904[280] = 0.0;
   out_3068454842243606904[281] = 0.0;
   out_3068454842243606904[282] = 0.0;
   out_3068454842243606904[283] = 0.0;
   out_3068454842243606904[284] = 0.0;
   out_3068454842243606904[285] = 0.0;
   out_3068454842243606904[286] = 0.0;
   out_3068454842243606904[287] = 0.0;
   out_3068454842243606904[288] = 0.0;
   out_3068454842243606904[289] = 0.0;
   out_3068454842243606904[290] = 0.0;
   out_3068454842243606904[291] = 0.0;
   out_3068454842243606904[292] = 0.0;
   out_3068454842243606904[293] = 0.0;
   out_3068454842243606904[294] = 0.0;
   out_3068454842243606904[295] = 0.0;
   out_3068454842243606904[296] = 0.0;
   out_3068454842243606904[297] = 0.0;
   out_3068454842243606904[298] = 1.0;
   out_3068454842243606904[299] = 0.0;
   out_3068454842243606904[300] = 0.0;
   out_3068454842243606904[301] = 0.0;
   out_3068454842243606904[302] = 0.0;
   out_3068454842243606904[303] = 0.0;
   out_3068454842243606904[304] = 0.0;
   out_3068454842243606904[305] = 0.0;
   out_3068454842243606904[306] = 0.0;
   out_3068454842243606904[307] = 0.0;
   out_3068454842243606904[308] = 0.0;
   out_3068454842243606904[309] = 0.0;
   out_3068454842243606904[310] = 0.0;
   out_3068454842243606904[311] = 0.0;
   out_3068454842243606904[312] = 0.0;
   out_3068454842243606904[313] = 0.0;
   out_3068454842243606904[314] = 0.0;
   out_3068454842243606904[315] = 0.0;
   out_3068454842243606904[316] = 0.0;
   out_3068454842243606904[317] = 0.0;
   out_3068454842243606904[318] = 0.0;
   out_3068454842243606904[319] = 0.0;
   out_3068454842243606904[320] = 0.0;
   out_3068454842243606904[321] = 1.0;
   out_3068454842243606904[322] = 0.0;
   out_3068454842243606904[323] = 0.0;
   out_3068454842243606904[324] = 0.0;
   out_3068454842243606904[325] = 0.0;
   out_3068454842243606904[326] = 0.0;
   out_3068454842243606904[327] = 0.0;
   out_3068454842243606904[328] = 0.0;
   out_3068454842243606904[329] = 0.0;
   out_3068454842243606904[330] = 0.0;
   out_3068454842243606904[331] = 0.0;
   out_3068454842243606904[332] = 0.0;
   out_3068454842243606904[333] = 0.0;
   out_3068454842243606904[334] = 0.0;
   out_3068454842243606904[335] = 0.0;
   out_3068454842243606904[336] = 0.0;
   out_3068454842243606904[337] = 0.0;
   out_3068454842243606904[338] = 0.0;
   out_3068454842243606904[339] = 0.0;
   out_3068454842243606904[340] = 0.0;
   out_3068454842243606904[341] = 0.0;
   out_3068454842243606904[342] = 0.0;
   out_3068454842243606904[343] = 0.0;
   out_3068454842243606904[344] = 1.0;
   out_3068454842243606904[345] = 0.0;
   out_3068454842243606904[346] = 0.0;
   out_3068454842243606904[347] = 0.0;
   out_3068454842243606904[348] = 0.0;
   out_3068454842243606904[349] = 0.0;
   out_3068454842243606904[350] = 0.0;
   out_3068454842243606904[351] = 0.0;
   out_3068454842243606904[352] = 0.0;
   out_3068454842243606904[353] = 0.0;
   out_3068454842243606904[354] = 0.0;
   out_3068454842243606904[355] = 0.0;
   out_3068454842243606904[356] = 0.0;
   out_3068454842243606904[357] = 0.0;
   out_3068454842243606904[358] = 0.0;
   out_3068454842243606904[359] = 0.0;
   out_3068454842243606904[360] = 0.0;
   out_3068454842243606904[361] = 0.0;
   out_3068454842243606904[362] = 0.0;
   out_3068454842243606904[363] = 0.0;
   out_3068454842243606904[364] = 0.0;
   out_3068454842243606904[365] = 0.0;
   out_3068454842243606904[366] = 0.0;
   out_3068454842243606904[367] = 1.0;
   out_3068454842243606904[368] = 0.0;
   out_3068454842243606904[369] = 0.0;
   out_3068454842243606904[370] = 0.0;
   out_3068454842243606904[371] = 0.0;
   out_3068454842243606904[372] = 0.0;
   out_3068454842243606904[373] = 0.0;
   out_3068454842243606904[374] = 0.0;
   out_3068454842243606904[375] = 0.0;
   out_3068454842243606904[376] = 0.0;
   out_3068454842243606904[377] = 0.0;
   out_3068454842243606904[378] = 0.0;
   out_3068454842243606904[379] = 0.0;
   out_3068454842243606904[380] = 0.0;
   out_3068454842243606904[381] = 0.0;
   out_3068454842243606904[382] = 0.0;
   out_3068454842243606904[383] = 0.0;
   out_3068454842243606904[384] = 0.0;
   out_3068454842243606904[385] = 0.0;
   out_3068454842243606904[386] = 0.0;
   out_3068454842243606904[387] = 0.0;
   out_3068454842243606904[388] = 0.0;
   out_3068454842243606904[389] = 0.0;
   out_3068454842243606904[390] = 1.0;
   out_3068454842243606904[391] = 0.0;
   out_3068454842243606904[392] = 0.0;
   out_3068454842243606904[393] = 0.0;
   out_3068454842243606904[394] = 0.0;
   out_3068454842243606904[395] = 0.0;
   out_3068454842243606904[396] = 0.0;
   out_3068454842243606904[397] = 0.0;
   out_3068454842243606904[398] = 0.0;
   out_3068454842243606904[399] = 0.0;
   out_3068454842243606904[400] = 0.0;
   out_3068454842243606904[401] = 0.0;
   out_3068454842243606904[402] = 0.0;
   out_3068454842243606904[403] = 0.0;
   out_3068454842243606904[404] = 0.0;
   out_3068454842243606904[405] = 0.0;
   out_3068454842243606904[406] = 0.0;
   out_3068454842243606904[407] = 0.0;
   out_3068454842243606904[408] = 0.0;
   out_3068454842243606904[409] = 0.0;
   out_3068454842243606904[410] = 0.0;
   out_3068454842243606904[411] = 0.0;
   out_3068454842243606904[412] = 0.0;
   out_3068454842243606904[413] = 1.0;
   out_3068454842243606904[414] = 0.0;
   out_3068454842243606904[415] = 0.0;
   out_3068454842243606904[416] = 0.0;
   out_3068454842243606904[417] = 0.0;
   out_3068454842243606904[418] = 0.0;
   out_3068454842243606904[419] = 0.0;
   out_3068454842243606904[420] = 0.0;
   out_3068454842243606904[421] = 0.0;
   out_3068454842243606904[422] = 0.0;
   out_3068454842243606904[423] = 0.0;
   out_3068454842243606904[424] = 0.0;
   out_3068454842243606904[425] = 0.0;
   out_3068454842243606904[426] = 0.0;
   out_3068454842243606904[427] = 0.0;
   out_3068454842243606904[428] = 0.0;
   out_3068454842243606904[429] = 0.0;
   out_3068454842243606904[430] = 0.0;
   out_3068454842243606904[431] = 0.0;
   out_3068454842243606904[432] = 0.0;
   out_3068454842243606904[433] = 0.0;
   out_3068454842243606904[434] = 0.0;
   out_3068454842243606904[435] = 0.0;
   out_3068454842243606904[436] = 1.0;
   out_3068454842243606904[437] = 0.0;
   out_3068454842243606904[438] = 0.0;
   out_3068454842243606904[439] = 0.0;
   out_3068454842243606904[440] = 0.0;
   out_3068454842243606904[441] = 0.0;
   out_3068454842243606904[442] = 0.0;
   out_3068454842243606904[443] = 0.0;
   out_3068454842243606904[444] = 0.0;
   out_3068454842243606904[445] = 0.0;
   out_3068454842243606904[446] = 0.0;
   out_3068454842243606904[447] = 0.0;
   out_3068454842243606904[448] = 0.0;
   out_3068454842243606904[449] = 0.0;
   out_3068454842243606904[450] = 0.0;
   out_3068454842243606904[451] = 0.0;
   out_3068454842243606904[452] = 0.0;
   out_3068454842243606904[453] = 0.0;
   out_3068454842243606904[454] = 0.0;
   out_3068454842243606904[455] = 0.0;
   out_3068454842243606904[456] = 0.0;
   out_3068454842243606904[457] = 0.0;
   out_3068454842243606904[458] = 0.0;
   out_3068454842243606904[459] = 1.0;
   out_3068454842243606904[460] = 0.0;
   out_3068454842243606904[461] = 0.0;
   out_3068454842243606904[462] = 0.0;
   out_3068454842243606904[463] = 0.0;
   out_3068454842243606904[464] = 0.0;
   out_3068454842243606904[465] = 0.0;
   out_3068454842243606904[466] = 0.0;
   out_3068454842243606904[467] = 0.0;
   out_3068454842243606904[468] = 0.0;
   out_3068454842243606904[469] = 0.0;
   out_3068454842243606904[470] = 0.0;
   out_3068454842243606904[471] = 0.0;
   out_3068454842243606904[472] = 0.0;
   out_3068454842243606904[473] = 0.0;
   out_3068454842243606904[474] = 0.0;
   out_3068454842243606904[475] = 0.0;
   out_3068454842243606904[476] = 0.0;
   out_3068454842243606904[477] = 0.0;
   out_3068454842243606904[478] = 0.0;
   out_3068454842243606904[479] = 0.0;
   out_3068454842243606904[480] = 0.0;
   out_3068454842243606904[481] = 0.0;
   out_3068454842243606904[482] = 1.0;
   out_3068454842243606904[483] = 0.0;
   out_3068454842243606904[484] = 0.0;
   out_3068454842243606904[485] = 0.0;
   out_3068454842243606904[486] = 0.0;
   out_3068454842243606904[487] = 0.0;
   out_3068454842243606904[488] = 0.0;
   out_3068454842243606904[489] = 0.0;
   out_3068454842243606904[490] = 0.0;
   out_3068454842243606904[491] = 0.0;
   out_3068454842243606904[492] = 0.0;
   out_3068454842243606904[493] = 0.0;
   out_3068454842243606904[494] = 0.0;
   out_3068454842243606904[495] = 0.0;
   out_3068454842243606904[496] = 0.0;
   out_3068454842243606904[497] = 0.0;
   out_3068454842243606904[498] = 0.0;
   out_3068454842243606904[499] = 0.0;
   out_3068454842243606904[500] = 0.0;
   out_3068454842243606904[501] = 0.0;
   out_3068454842243606904[502] = 0.0;
   out_3068454842243606904[503] = 0.0;
   out_3068454842243606904[504] = 0.0;
   out_3068454842243606904[505] = 1.0;
}
void f_fun(double *state, double dt, double *out_6548865548862311877) {
   out_6548865548862311877[0] = dt*state[7] + state[0];
   out_6548865548862311877[1] = dt*state[8] + state[1];
   out_6548865548862311877[2] = dt*state[9] + state[2];
   out_6548865548862311877[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3];
   out_6548865548862311877[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4];
   out_6548865548862311877[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5];
   out_6548865548862311877[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6];
   out_6548865548862311877[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[19] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[18] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[7];
   out_6548865548862311877[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[19] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[18]) + state[8];
   out_6548865548862311877[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[17] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[19]) + state[9];
   out_6548865548862311877[10] = state[10];
   out_6548865548862311877[11] = state[11];
   out_6548865548862311877[12] = state[12];
   out_6548865548862311877[13] = state[13];
   out_6548865548862311877[14] = state[14];
   out_6548865548862311877[15] = state[15];
   out_6548865548862311877[16] = state[16];
   out_6548865548862311877[17] = state[17];
   out_6548865548862311877[18] = state[18];
   out_6548865548862311877[19] = state[19];
   out_6548865548862311877[20] = state[20];
   out_6548865548862311877[21] = state[21];
   out_6548865548862311877[22] = state[22];
}
void F_fun(double *state, double dt, double *out_2795244210510601530) {
   out_2795244210510601530[0] = 1;
   out_2795244210510601530[1] = 0;
   out_2795244210510601530[2] = 0;
   out_2795244210510601530[3] = 0;
   out_2795244210510601530[4] = 0;
   out_2795244210510601530[5] = 0;
   out_2795244210510601530[6] = dt;
   out_2795244210510601530[7] = 0;
   out_2795244210510601530[8] = 0;
   out_2795244210510601530[9] = 0;
   out_2795244210510601530[10] = 0;
   out_2795244210510601530[11] = 0;
   out_2795244210510601530[12] = 0;
   out_2795244210510601530[13] = 0;
   out_2795244210510601530[14] = 0;
   out_2795244210510601530[15] = 0;
   out_2795244210510601530[16] = 0;
   out_2795244210510601530[17] = 0;
   out_2795244210510601530[18] = 0;
   out_2795244210510601530[19] = 0;
   out_2795244210510601530[20] = 0;
   out_2795244210510601530[21] = 0;
   out_2795244210510601530[22] = 0;
   out_2795244210510601530[23] = 1;
   out_2795244210510601530[24] = 0;
   out_2795244210510601530[25] = 0;
   out_2795244210510601530[26] = 0;
   out_2795244210510601530[27] = 0;
   out_2795244210510601530[28] = 0;
   out_2795244210510601530[29] = dt;
   out_2795244210510601530[30] = 0;
   out_2795244210510601530[31] = 0;
   out_2795244210510601530[32] = 0;
   out_2795244210510601530[33] = 0;
   out_2795244210510601530[34] = 0;
   out_2795244210510601530[35] = 0;
   out_2795244210510601530[36] = 0;
   out_2795244210510601530[37] = 0;
   out_2795244210510601530[38] = 0;
   out_2795244210510601530[39] = 0;
   out_2795244210510601530[40] = 0;
   out_2795244210510601530[41] = 0;
   out_2795244210510601530[42] = 0;
   out_2795244210510601530[43] = 0;
   out_2795244210510601530[44] = 0;
   out_2795244210510601530[45] = 0;
   out_2795244210510601530[46] = 1;
   out_2795244210510601530[47] = 0;
   out_2795244210510601530[48] = 0;
   out_2795244210510601530[49] = 0;
   out_2795244210510601530[50] = 0;
   out_2795244210510601530[51] = 0;
   out_2795244210510601530[52] = dt;
   out_2795244210510601530[53] = 0;
   out_2795244210510601530[54] = 0;
   out_2795244210510601530[55] = 0;
   out_2795244210510601530[56] = 0;
   out_2795244210510601530[57] = 0;
   out_2795244210510601530[58] = 0;
   out_2795244210510601530[59] = 0;
   out_2795244210510601530[60] = 0;
   out_2795244210510601530[61] = 0;
   out_2795244210510601530[62] = 0;
   out_2795244210510601530[63] = 0;
   out_2795244210510601530[64] = 0;
   out_2795244210510601530[65] = 0;
   out_2795244210510601530[66] = 0;
   out_2795244210510601530[67] = 0;
   out_2795244210510601530[68] = 0;
   out_2795244210510601530[69] = 1;
   out_2795244210510601530[70] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
   out_2795244210510601530[71] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]);
   out_2795244210510601530[72] = 0;
   out_2795244210510601530[73] = 0;
   out_2795244210510601530[74] = 0;
   out_2795244210510601530[75] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
   out_2795244210510601530[76] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
   out_2795244210510601530[77] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
   out_2795244210510601530[78] = 0;
   out_2795244210510601530[79] = 0;
   out_2795244210510601530[80] = 0;
   out_2795244210510601530[81] = 0;
   out_2795244210510601530[82] = 0;
   out_2795244210510601530[83] = 0;
   out_2795244210510601530[84] = 0;
   out_2795244210510601530[85] = 0;
   out_2795244210510601530[86] = 0;
   out_2795244210510601530[87] = 0;
   out_2795244210510601530[88] = 0;
   out_2795244210510601530[89] = 0;
   out_2795244210510601530[90] = 0;
   out_2795244210510601530[91] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]);
   out_2795244210510601530[92] = 1;
   out_2795244210510601530[93] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]);
   out_2795244210510601530[94] = 0;
   out_2795244210510601530[95] = 0;
   out_2795244210510601530[96] = 0;
   out_2795244210510601530[97] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
   out_2795244210510601530[98] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
   out_2795244210510601530[99] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
   out_2795244210510601530[100] = 0;
   out_2795244210510601530[101] = 0;
   out_2795244210510601530[102] = 0;
   out_2795244210510601530[103] = 0;
   out_2795244210510601530[104] = 0;
   out_2795244210510601530[105] = 0;
   out_2795244210510601530[106] = 0;
   out_2795244210510601530[107] = 0;
   out_2795244210510601530[108] = 0;
   out_2795244210510601530[109] = 0;
   out_2795244210510601530[110] = 0;
   out_2795244210510601530[111] = 0;
   out_2795244210510601530[112] = 0;
   out_2795244210510601530[113] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]);
   out_2795244210510601530[114] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]);
   out_2795244210510601530[115] = 1;
   out_2795244210510601530[116] = 0;
   out_2795244210510601530[117] = 0;
   out_2795244210510601530[118] = 0;
   out_2795244210510601530[119] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
   out_2795244210510601530[120] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
   out_2795244210510601530[121] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
   out_2795244210510601530[122] = 0;
   out_2795244210510601530[123] = 0;
   out_2795244210510601530[124] = 0;
   out_2795244210510601530[125] = 0;
   out_2795244210510601530[126] = 0;
   out_2795244210510601530[127] = 0;
   out_2795244210510601530[128] = 0;
   out_2795244210510601530[129] = 0;
   out_2795244210510601530[130] = 0;
   out_2795244210510601530[131] = 0;
   out_2795244210510601530[132] = 0;
   out_2795244210510601530[133] = 0;
   out_2795244210510601530[134] = 0;
   out_2795244210510601530[135] = 0;
   out_2795244210510601530[136] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[17] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[19]);
   out_2795244210510601530[137] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[19] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]);
   out_2795244210510601530[138] = 1;
   out_2795244210510601530[139] = 0;
   out_2795244210510601530[140] = 0;
   out_2795244210510601530[141] = 0;
   out_2795244210510601530[142] = 0;
   out_2795244210510601530[143] = 0;
   out_2795244210510601530[144] = 0;
   out_2795244210510601530[145] = 0;
   out_2795244210510601530[146] = 0;
   out_2795244210510601530[147] = 0;
   out_2795244210510601530[148] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
   out_2795244210510601530[149] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]);
   out_2795244210510601530[150] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]);
   out_2795244210510601530[151] = 0;
   out_2795244210510601530[152] = 0;
   out_2795244210510601530[153] = 0;
   out_2795244210510601530[154] = 0;
   out_2795244210510601530[155] = 0;
   out_2795244210510601530[156] = 0;
   out_2795244210510601530[157] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[18] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[17] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[19]);
   out_2795244210510601530[158] = 0;
   out_2795244210510601530[159] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[19] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[18] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[17]);
   out_2795244210510601530[160] = 0;
   out_2795244210510601530[161] = 1;
   out_2795244210510601530[162] = 0;
   out_2795244210510601530[163] = 0;
   out_2795244210510601530[164] = 0;
   out_2795244210510601530[165] = 0;
   out_2795244210510601530[166] = 0;
   out_2795244210510601530[167] = 0;
   out_2795244210510601530[168] = 0;
   out_2795244210510601530[169] = 0;
   out_2795244210510601530[170] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]);
   out_2795244210510601530[171] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
   out_2795244210510601530[172] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]);
   out_2795244210510601530[173] = 0;
   out_2795244210510601530[174] = 0;
   out_2795244210510601530[175] = 0;
   out_2795244210510601530[176] = 0;
   out_2795244210510601530[177] = 0;
   out_2795244210510601530[178] = 0;
   out_2795244210510601530[179] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[19] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[18]);
   out_2795244210510601530[180] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[19] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[18] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[17]);
   out_2795244210510601530[181] = 0;
   out_2795244210510601530[182] = 0;
   out_2795244210510601530[183] = 0;
   out_2795244210510601530[184] = 1;
   out_2795244210510601530[185] = 0;
   out_2795244210510601530[186] = 0;
   out_2795244210510601530[187] = 0;
   out_2795244210510601530[188] = 0;
   out_2795244210510601530[189] = 0;
   out_2795244210510601530[190] = 0;
   out_2795244210510601530[191] = 0;
   out_2795244210510601530[192] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]);
   out_2795244210510601530[193] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]);
   out_2795244210510601530[194] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
   out_2795244210510601530[195] = 0;
   out_2795244210510601530[196] = 0;
   out_2795244210510601530[197] = 0;
   out_2795244210510601530[198] = 0;
   out_2795244210510601530[199] = 0;
   out_2795244210510601530[200] = 0;
   out_2795244210510601530[201] = 0;
   out_2795244210510601530[202] = 0;
   out_2795244210510601530[203] = 0;
   out_2795244210510601530[204] = 0;
   out_2795244210510601530[205] = 0;
   out_2795244210510601530[206] = 0;
   out_2795244210510601530[207] = 1;
   out_2795244210510601530[208] = 0;
   out_2795244210510601530[209] = 0;
   out_2795244210510601530[210] = 0;
   out_2795244210510601530[211] = 0;
   out_2795244210510601530[212] = 0;
   out_2795244210510601530[213] = 0;
   out_2795244210510601530[214] = 0;
   out_2795244210510601530[215] = 0;
   out_2795244210510601530[216] = 0;
   out_2795244210510601530[217] = 0;
   out_2795244210510601530[218] = 0;
   out_2795244210510601530[219] = 0;
   out_2795244210510601530[220] = 0;
   out_2795244210510601530[221] = 0;
   out_2795244210510601530[222] = 0;
   out_2795244210510601530[223] = 0;
   out_2795244210510601530[224] = 0;
   out_2795244210510601530[225] = 0;
   out_2795244210510601530[226] = 0;
   out_2795244210510601530[227] = 0;
   out_2795244210510601530[228] = 0;
   out_2795244210510601530[229] = 0;
   out_2795244210510601530[230] = 1;
   out_2795244210510601530[231] = 0;
   out_2795244210510601530[232] = 0;
   out_2795244210510601530[233] = 0;
   out_2795244210510601530[234] = 0;
   out_2795244210510601530[235] = 0;
   out_2795244210510601530[236] = 0;
   out_2795244210510601530[237] = 0;
   out_2795244210510601530[238] = 0;
   out_2795244210510601530[239] = 0;
   out_2795244210510601530[240] = 0;
   out_2795244210510601530[241] = 0;
   out_2795244210510601530[242] = 0;
   out_2795244210510601530[243] = 0;
   out_2795244210510601530[244] = 0;
   out_2795244210510601530[245] = 0;
   out_2795244210510601530[246] = 0;
   out_2795244210510601530[247] = 0;
   out_2795244210510601530[248] = 0;
   out_2795244210510601530[249] = 0;
   out_2795244210510601530[250] = 0;
   out_2795244210510601530[251] = 0;
   out_2795244210510601530[252] = 0;
   out_2795244210510601530[253] = 1;
   out_2795244210510601530[254] = 0;
   out_2795244210510601530[255] = 0;
   out_2795244210510601530[256] = 0;
   out_2795244210510601530[257] = 0;
   out_2795244210510601530[258] = 0;
   out_2795244210510601530[259] = 0;
   out_2795244210510601530[260] = 0;
   out_2795244210510601530[261] = 0;
   out_2795244210510601530[262] = 0;
   out_2795244210510601530[263] = 0;
   out_2795244210510601530[264] = 0;
   out_2795244210510601530[265] = 0;
   out_2795244210510601530[266] = 0;
   out_2795244210510601530[267] = 0;
   out_2795244210510601530[268] = 0;
   out_2795244210510601530[269] = 0;
   out_2795244210510601530[270] = 0;
   out_2795244210510601530[271] = 0;
   out_2795244210510601530[272] = 0;
   out_2795244210510601530[273] = 0;
   out_2795244210510601530[274] = 0;
   out_2795244210510601530[275] = 0;
   out_2795244210510601530[276] = 1;
   out_2795244210510601530[277] = 0;
   out_2795244210510601530[278] = 0;
   out_2795244210510601530[279] = 0;
   out_2795244210510601530[280] = 0;
   out_2795244210510601530[281] = 0;
   out_2795244210510601530[282] = 0;
   out_2795244210510601530[283] = 0;
   out_2795244210510601530[284] = 0;
   out_2795244210510601530[285] = 0;
   out_2795244210510601530[286] = 0;
   out_2795244210510601530[287] = 0;
   out_2795244210510601530[288] = 0;
   out_2795244210510601530[289] = 0;
   out_2795244210510601530[290] = 0;
   out_2795244210510601530[291] = 0;
   out_2795244210510601530[292] = 0;
   out_2795244210510601530[293] = 0;
   out_2795244210510601530[294] = 0;
   out_2795244210510601530[295] = 0;
   out_2795244210510601530[296] = 0;
   out_2795244210510601530[297] = 0;
   out_2795244210510601530[298] = 0;
   out_2795244210510601530[299] = 1;
   out_2795244210510601530[300] = 0;
   out_2795244210510601530[301] = 0;
   out_2795244210510601530[302] = 0;
   out_2795244210510601530[303] = 0;
   out_2795244210510601530[304] = 0;
   out_2795244210510601530[305] = 0;
   out_2795244210510601530[306] = 0;
   out_2795244210510601530[307] = 0;
   out_2795244210510601530[308] = 0;
   out_2795244210510601530[309] = 0;
   out_2795244210510601530[310] = 0;
   out_2795244210510601530[311] = 0;
   out_2795244210510601530[312] = 0;
   out_2795244210510601530[313] = 0;
   out_2795244210510601530[314] = 0;
   out_2795244210510601530[315] = 0;
   out_2795244210510601530[316] = 0;
   out_2795244210510601530[317] = 0;
   out_2795244210510601530[318] = 0;
   out_2795244210510601530[319] = 0;
   out_2795244210510601530[320] = 0;
   out_2795244210510601530[321] = 0;
   out_2795244210510601530[322] = 1;
   out_2795244210510601530[323] = 0;
   out_2795244210510601530[324] = 0;
   out_2795244210510601530[325] = 0;
   out_2795244210510601530[326] = 0;
   out_2795244210510601530[327] = 0;
   out_2795244210510601530[328] = 0;
   out_2795244210510601530[329] = 0;
   out_2795244210510601530[330] = 0;
   out_2795244210510601530[331] = 0;
   out_2795244210510601530[332] = 0;
   out_2795244210510601530[333] = 0;
   out_2795244210510601530[334] = 0;
   out_2795244210510601530[335] = 0;
   out_2795244210510601530[336] = 0;
   out_2795244210510601530[337] = 0;
   out_2795244210510601530[338] = 0;
   out_2795244210510601530[339] = 0;
   out_2795244210510601530[340] = 0;
   out_2795244210510601530[341] = 0;
   out_2795244210510601530[342] = 0;
   out_2795244210510601530[343] = 0;
   out_2795244210510601530[344] = 0;
   out_2795244210510601530[345] = 1;
   out_2795244210510601530[346] = 0;
   out_2795244210510601530[347] = 0;
   out_2795244210510601530[348] = 0;
   out_2795244210510601530[349] = 0;
   out_2795244210510601530[350] = 0;
   out_2795244210510601530[351] = 0;
   out_2795244210510601530[352] = 0;
   out_2795244210510601530[353] = 0;
   out_2795244210510601530[354] = 0;
   out_2795244210510601530[355] = 0;
   out_2795244210510601530[356] = 0;
   out_2795244210510601530[357] = 0;
   out_2795244210510601530[358] = 0;
   out_2795244210510601530[359] = 0;
   out_2795244210510601530[360] = 0;
   out_2795244210510601530[361] = 0;
   out_2795244210510601530[362] = 0;
   out_2795244210510601530[363] = 0;
   out_2795244210510601530[364] = 0;
   out_2795244210510601530[365] = 0;
   out_2795244210510601530[366] = 0;
   out_2795244210510601530[367] = 0;
   out_2795244210510601530[368] = 1;
   out_2795244210510601530[369] = 0;
   out_2795244210510601530[370] = 0;
   out_2795244210510601530[371] = 0;
   out_2795244210510601530[372] = 0;
   out_2795244210510601530[373] = 0;
   out_2795244210510601530[374] = 0;
   out_2795244210510601530[375] = 0;
   out_2795244210510601530[376] = 0;
   out_2795244210510601530[377] = 0;
   out_2795244210510601530[378] = 0;
   out_2795244210510601530[379] = 0;
   out_2795244210510601530[380] = 0;
   out_2795244210510601530[381] = 0;
   out_2795244210510601530[382] = 0;
   out_2795244210510601530[383] = 0;
   out_2795244210510601530[384] = 0;
   out_2795244210510601530[385] = 0;
   out_2795244210510601530[386] = 0;
   out_2795244210510601530[387] = 0;
   out_2795244210510601530[388] = 0;
   out_2795244210510601530[389] = 0;
   out_2795244210510601530[390] = 0;
   out_2795244210510601530[391] = 1;
   out_2795244210510601530[392] = 0;
   out_2795244210510601530[393] = 0;
   out_2795244210510601530[394] = 0;
   out_2795244210510601530[395] = 0;
   out_2795244210510601530[396] = 0;
   out_2795244210510601530[397] = 0;
   out_2795244210510601530[398] = 0;
   out_2795244210510601530[399] = 0;
   out_2795244210510601530[400] = 0;
   out_2795244210510601530[401] = 0;
   out_2795244210510601530[402] = 0;
   out_2795244210510601530[403] = 0;
   out_2795244210510601530[404] = 0;
   out_2795244210510601530[405] = 0;
   out_2795244210510601530[406] = 0;
   out_2795244210510601530[407] = 0;
   out_2795244210510601530[408] = 0;
   out_2795244210510601530[409] = 0;
   out_2795244210510601530[410] = 0;
   out_2795244210510601530[411] = 0;
   out_2795244210510601530[412] = 0;
   out_2795244210510601530[413] = 0;
   out_2795244210510601530[414] = 1;
   out_2795244210510601530[415] = 0;
   out_2795244210510601530[416] = 0;
   out_2795244210510601530[417] = 0;
   out_2795244210510601530[418] = 0;
   out_2795244210510601530[419] = 0;
   out_2795244210510601530[420] = 0;
   out_2795244210510601530[421] = 0;
   out_2795244210510601530[422] = 0;
   out_2795244210510601530[423] = 0;
   out_2795244210510601530[424] = 0;
   out_2795244210510601530[425] = 0;
   out_2795244210510601530[426] = 0;
   out_2795244210510601530[427] = 0;
   out_2795244210510601530[428] = 0;
   out_2795244210510601530[429] = 0;
   out_2795244210510601530[430] = 0;
   out_2795244210510601530[431] = 0;
   out_2795244210510601530[432] = 0;
   out_2795244210510601530[433] = 0;
   out_2795244210510601530[434] = 0;
   out_2795244210510601530[435] = 0;
   out_2795244210510601530[436] = 0;
   out_2795244210510601530[437] = 1;
   out_2795244210510601530[438] = 0;
   out_2795244210510601530[439] = 0;
   out_2795244210510601530[440] = 0;
   out_2795244210510601530[441] = 0;
   out_2795244210510601530[442] = 0;
   out_2795244210510601530[443] = 0;
   out_2795244210510601530[444] = 0;
   out_2795244210510601530[445] = 0;
   out_2795244210510601530[446] = 0;
   out_2795244210510601530[447] = 0;
   out_2795244210510601530[448] = 0;
   out_2795244210510601530[449] = 0;
   out_2795244210510601530[450] = 0;
   out_2795244210510601530[451] = 0;
   out_2795244210510601530[452] = 0;
   out_2795244210510601530[453] = 0;
   out_2795244210510601530[454] = 0;
   out_2795244210510601530[455] = 0;
   out_2795244210510601530[456] = 0;
   out_2795244210510601530[457] = 0;
   out_2795244210510601530[458] = 0;
   out_2795244210510601530[459] = 0;
   out_2795244210510601530[460] = 1;
   out_2795244210510601530[461] = 0;
   out_2795244210510601530[462] = 0;
   out_2795244210510601530[463] = 0;
   out_2795244210510601530[464] = 0;
   out_2795244210510601530[465] = 0;
   out_2795244210510601530[466] = 0;
   out_2795244210510601530[467] = 0;
   out_2795244210510601530[468] = 0;
   out_2795244210510601530[469] = 0;
   out_2795244210510601530[470] = 0;
   out_2795244210510601530[471] = 0;
   out_2795244210510601530[472] = 0;
   out_2795244210510601530[473] = 0;
   out_2795244210510601530[474] = 0;
   out_2795244210510601530[475] = 0;
   out_2795244210510601530[476] = 0;
   out_2795244210510601530[477] = 0;
   out_2795244210510601530[478] = 0;
   out_2795244210510601530[479] = 0;
   out_2795244210510601530[480] = 0;
   out_2795244210510601530[481] = 0;
   out_2795244210510601530[482] = 0;
   out_2795244210510601530[483] = 1;
}
void h_3(double *state, double *unused, double *out_5890016392798834445) {
   out_5890016392798834445[0] = sqrt(pow(state[7], 2) + pow(state[8], 2) + pow(state[9], 2) + 9.9999999999999995e-7)*state[16];
}
void H_3(double *state, double *unused, double *out_5951574930511872297) {
   out_5951574930511872297[0] = 0;
   out_5951574930511872297[1] = 0;
   out_5951574930511872297[2] = 0;
   out_5951574930511872297[3] = 0;
   out_5951574930511872297[4] = 0;
   out_5951574930511872297[5] = 0;
   out_5951574930511872297[6] = 0;
   out_5951574930511872297[7] = state[7]*state[16]/sqrt(pow(state[7], 2) + pow(state[8], 2) + pow(state[9], 2) + 9.9999999999999995e-7);
   out_5951574930511872297[8] = state[8]*state[16]/sqrt(pow(state[7], 2) + pow(state[8], 2) + pow(state[9], 2) + 9.9999999999999995e-7);
   out_5951574930511872297[9] = state[9]*state[16]/sqrt(pow(state[7], 2) + pow(state[8], 2) + pow(state[9], 2) + 9.9999999999999995e-7);
   out_5951574930511872297[10] = 0;
   out_5951574930511872297[11] = 0;
   out_5951574930511872297[12] = 0;
   out_5951574930511872297[13] = 0;
   out_5951574930511872297[14] = 0;
   out_5951574930511872297[15] = 0;
   out_5951574930511872297[16] = sqrt(pow(state[7], 2) + pow(state[8], 2) + pow(state[9], 2) + 9.9999999999999995e-7);
   out_5951574930511872297[17] = 0;
   out_5951574930511872297[18] = 0;
   out_5951574930511872297[19] = 0;
   out_5951574930511872297[20] = 0;
   out_5951574930511872297[21] = 0;
   out_5951574930511872297[22] = 0;
}
void h_4(double *state, double *unused, double *out_8526004289681937483) {
   out_8526004289681937483[0] = state[10] + state[13];
   out_8526004289681937483[1] = state[11] + state[14];
   out_8526004289681937483[2] = state[12] + state[15];
}
void H_4(double *state, double *unused, double *out_2015221744362692841) {
   out_2015221744362692841[0] = 0;
   out_2015221744362692841[1] = 0;
   out_2015221744362692841[2] = 0;
   out_2015221744362692841[3] = 0;
   out_2015221744362692841[4] = 0;
   out_2015221744362692841[5] = 0;
   out_2015221744362692841[6] = 0;
   out_2015221744362692841[7] = 0;
   out_2015221744362692841[8] = 0;
   out_2015221744362692841[9] = 0;
   out_2015221744362692841[10] = 1;
   out_2015221744362692841[11] = 0;
   out_2015221744362692841[12] = 0;
   out_2015221744362692841[13] = 1;
   out_2015221744362692841[14] = 0;
   out_2015221744362692841[15] = 0;
   out_2015221744362692841[16] = 0;
   out_2015221744362692841[17] = 0;
   out_2015221744362692841[18] = 0;
   out_2015221744362692841[19] = 0;
   out_2015221744362692841[20] = 0;
   out_2015221744362692841[21] = 0;
   out_2015221744362692841[22] = 0;
   out_2015221744362692841[23] = 0;
   out_2015221744362692841[24] = 0;
   out_2015221744362692841[25] = 0;
   out_2015221744362692841[26] = 0;
   out_2015221744362692841[27] = 0;
   out_2015221744362692841[28] = 0;
   out_2015221744362692841[29] = 0;
   out_2015221744362692841[30] = 0;
   out_2015221744362692841[31] = 0;
   out_2015221744362692841[32] = 0;
   out_2015221744362692841[33] = 0;
   out_2015221744362692841[34] = 1;
   out_2015221744362692841[35] = 0;
   out_2015221744362692841[36] = 0;
   out_2015221744362692841[37] = 1;
   out_2015221744362692841[38] = 0;
   out_2015221744362692841[39] = 0;
   out_2015221744362692841[40] = 0;
   out_2015221744362692841[41] = 0;
   out_2015221744362692841[42] = 0;
   out_2015221744362692841[43] = 0;
   out_2015221744362692841[44] = 0;
   out_2015221744362692841[45] = 0;
   out_2015221744362692841[46] = 0;
   out_2015221744362692841[47] = 0;
   out_2015221744362692841[48] = 0;
   out_2015221744362692841[49] = 0;
   out_2015221744362692841[50] = 0;
   out_2015221744362692841[51] = 0;
   out_2015221744362692841[52] = 0;
   out_2015221744362692841[53] = 0;
   out_2015221744362692841[54] = 0;
   out_2015221744362692841[55] = 0;
   out_2015221744362692841[56] = 0;
   out_2015221744362692841[57] = 0;
   out_2015221744362692841[58] = 1;
   out_2015221744362692841[59] = 0;
   out_2015221744362692841[60] = 0;
   out_2015221744362692841[61] = 1;
   out_2015221744362692841[62] = 0;
   out_2015221744362692841[63] = 0;
   out_2015221744362692841[64] = 0;
   out_2015221744362692841[65] = 0;
   out_2015221744362692841[66] = 0;
   out_2015221744362692841[67] = 0;
   out_2015221744362692841[68] = 0;
}
void h_9(double *state, double *unused, double *out_6849354457489368967) {
   out_6849354457489368967[0] = state[10];
   out_6849354457489368967[1] = state[11];
   out_6849354457489368967[2] = state[12];
}
void H_9(double *state, double *unused, double *out_7925197860050783002) {
   out_7925197860050783002[0] = 0;
   out_7925197860050783002[1] = 0;
   out_7925197860050783002[2] = 0;
   out_7925197860050783002[3] = 0;
   out_7925197860050783002[4] = 0;
   out_7925197860050783002[5] = 0;
   out_7925197860050783002[6] = 0;
   out_7925197860050783002[7] = 0;
   out_7925197860050783002[8] = 0;
   out_7925197860050783002[9] = 0;
   out_7925197860050783002[10] = 1;
   out_7925197860050783002[11] = 0;
   out_7925197860050783002[12] = 0;
   out_7925197860050783002[13] = 0;
   out_7925197860050783002[14] = 0;
   out_7925197860050783002[15] = 0;
   out_7925197860050783002[16] = 0;
   out_7925197860050783002[17] = 0;
   out_7925197860050783002[18] = 0;
   out_7925197860050783002[19] = 0;
   out_7925197860050783002[20] = 0;
   out_7925197860050783002[21] = 0;
   out_7925197860050783002[22] = 0;
   out_7925197860050783002[23] = 0;
   out_7925197860050783002[24] = 0;
   out_7925197860050783002[25] = 0;
   out_7925197860050783002[26] = 0;
   out_7925197860050783002[27] = 0;
   out_7925197860050783002[28] = 0;
   out_7925197860050783002[29] = 0;
   out_7925197860050783002[30] = 0;
   out_7925197860050783002[31] = 0;
   out_7925197860050783002[32] = 0;
   out_7925197860050783002[33] = 0;
   out_7925197860050783002[34] = 1;
   out_7925197860050783002[35] = 0;
   out_7925197860050783002[36] = 0;
   out_7925197860050783002[37] = 0;
   out_7925197860050783002[38] = 0;
   out_7925197860050783002[39] = 0;
   out_7925197860050783002[40] = 0;
   out_7925197860050783002[41] = 0;
   out_7925197860050783002[42] = 0;
   out_7925197860050783002[43] = 0;
   out_7925197860050783002[44] = 0;
   out_7925197860050783002[45] = 0;
   out_7925197860050783002[46] = 0;
   out_7925197860050783002[47] = 0;
   out_7925197860050783002[48] = 0;
   out_7925197860050783002[49] = 0;
   out_7925197860050783002[50] = 0;
   out_7925197860050783002[51] = 0;
   out_7925197860050783002[52] = 0;
   out_7925197860050783002[53] = 0;
   out_7925197860050783002[54] = 0;
   out_7925197860050783002[55] = 0;
   out_7925197860050783002[56] = 0;
   out_7925197860050783002[57] = 0;
   out_7925197860050783002[58] = 1;
   out_7925197860050783002[59] = 0;
   out_7925197860050783002[60] = 0;
   out_7925197860050783002[61] = 0;
   out_7925197860050783002[62] = 0;
   out_7925197860050783002[63] = 0;
   out_7925197860050783002[64] = 0;
   out_7925197860050783002[65] = 0;
   out_7925197860050783002[66] = 0;
   out_7925197860050783002[67] = 0;
   out_7925197860050783002[68] = 0;
}
void h_10(double *state, double *unused, double *out_6748519270298518309) {
   out_6748519270298518309[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[17];
   out_6748519270298518309[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[18];
   out_6748519270298518309[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[19];
}
void H_10(double *state, double *unused, double *out_409868221626246429) {
   out_409868221626246429[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2));
   out_409868221626246429[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
   out_409868221626246429[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2];
   out_409868221626246429[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
   out_409868221626246429[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
   out_409868221626246429[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
   out_409868221626246429[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
   out_409868221626246429[7] = 0;
   out_409868221626246429[8] = 0;
   out_409868221626246429[9] = 0;
   out_409868221626246429[10] = 0;
   out_409868221626246429[11] = 0;
   out_409868221626246429[12] = 0;
   out_409868221626246429[13] = 0;
   out_409868221626246429[14] = 0;
   out_409868221626246429[15] = 0;
   out_409868221626246429[16] = 0;
   out_409868221626246429[17] = 1;
   out_409868221626246429[18] = 0;
   out_409868221626246429[19] = 0;
   out_409868221626246429[20] = 0;
   out_409868221626246429[21] = 0;
   out_409868221626246429[22] = 0;
   out_409868221626246429[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1];
   out_409868221626246429[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2));
   out_409868221626246429[25] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2];
   out_409868221626246429[26] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
   out_409868221626246429[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
   out_409868221626246429[28] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
   out_409868221626246429[29] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
   out_409868221626246429[30] = 0;
   out_409868221626246429[31] = 0;
   out_409868221626246429[32] = 0;
   out_409868221626246429[33] = 0;
   out_409868221626246429[34] = 0;
   out_409868221626246429[35] = 0;
   out_409868221626246429[36] = 0;
   out_409868221626246429[37] = 0;
   out_409868221626246429[38] = 0;
   out_409868221626246429[39] = 0;
   out_409868221626246429[40] = 0;
   out_409868221626246429[41] = 1;
   out_409868221626246429[42] = 0;
   out_409868221626246429[43] = 0;
   out_409868221626246429[44] = 0;
   out_409868221626246429[45] = 0;
   out_409868221626246429[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2];
   out_409868221626246429[47] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2];
   out_409868221626246429[48] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2));
   out_409868221626246429[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3];
   out_409868221626246429[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4];
   out_409868221626246429[51] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5];
   out_409868221626246429[52] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6];
   out_409868221626246429[53] = 0;
   out_409868221626246429[54] = 0;
   out_409868221626246429[55] = 0;
   out_409868221626246429[56] = 0;
   out_409868221626246429[57] = 0;
   out_409868221626246429[58] = 0;
   out_409868221626246429[59] = 0;
   out_409868221626246429[60] = 0;
   out_409868221626246429[61] = 0;
   out_409868221626246429[62] = 0;
   out_409868221626246429[63] = 0;
   out_409868221626246429[64] = 0;
   out_409868221626246429[65] = 1;
   out_409868221626246429[66] = 0;
   out_409868221626246429[67] = 0;
   out_409868221626246429[68] = 0;
}
void h_12(double *state, double *unused, double *out_1066610582664881043) {
   out_1066610582664881043[0] = state[0];
   out_1066610582664881043[1] = state[1];
   out_1066610582664881043[2] = state[2];
}
void H_12(double *state, double *unused, double *out_6435812489740243594) {
   out_6435812489740243594[0] = 1;
   out_6435812489740243594[1] = 0;
   out_6435812489740243594[2] = 0;
   out_6435812489740243594[3] = 0;
   out_6435812489740243594[4] = 0;
   out_6435812489740243594[5] = 0;
   out_6435812489740243594[6] = 0;
   out_6435812489740243594[7] = 0;
   out_6435812489740243594[8] = 0;
   out_6435812489740243594[9] = 0;
   out_6435812489740243594[10] = 0;
   out_6435812489740243594[11] = 0;
   out_6435812489740243594[12] = 0;
   out_6435812489740243594[13] = 0;
   out_6435812489740243594[14] = 0;
   out_6435812489740243594[15] = 0;
   out_6435812489740243594[16] = 0;
   out_6435812489740243594[17] = 0;
   out_6435812489740243594[18] = 0;
   out_6435812489740243594[19] = 0;
   out_6435812489740243594[20] = 0;
   out_6435812489740243594[21] = 0;
   out_6435812489740243594[22] = 0;
   out_6435812489740243594[23] = 0;
   out_6435812489740243594[24] = 1;
   out_6435812489740243594[25] = 0;
   out_6435812489740243594[26] = 0;
   out_6435812489740243594[27] = 0;
   out_6435812489740243594[28] = 0;
   out_6435812489740243594[29] = 0;
   out_6435812489740243594[30] = 0;
   out_6435812489740243594[31] = 0;
   out_6435812489740243594[32] = 0;
   out_6435812489740243594[33] = 0;
   out_6435812489740243594[34] = 0;
   out_6435812489740243594[35] = 0;
   out_6435812489740243594[36] = 0;
   out_6435812489740243594[37] = 0;
   out_6435812489740243594[38] = 0;
   out_6435812489740243594[39] = 0;
   out_6435812489740243594[40] = 0;
   out_6435812489740243594[41] = 0;
   out_6435812489740243594[42] = 0;
   out_6435812489740243594[43] = 0;
   out_6435812489740243594[44] = 0;
   out_6435812489740243594[45] = 0;
   out_6435812489740243594[46] = 0;
   out_6435812489740243594[47] = 0;
   out_6435812489740243594[48] = 1;
   out_6435812489740243594[49] = 0;
   out_6435812489740243594[50] = 0;
   out_6435812489740243594[51] = 0;
   out_6435812489740243594[52] = 0;
   out_6435812489740243594[53] = 0;
   out_6435812489740243594[54] = 0;
   out_6435812489740243594[55] = 0;
   out_6435812489740243594[56] = 0;
   out_6435812489740243594[57] = 0;
   out_6435812489740243594[58] = 0;
   out_6435812489740243594[59] = 0;
   out_6435812489740243594[60] = 0;
   out_6435812489740243594[61] = 0;
   out_6435812489740243594[62] = 0;
   out_6435812489740243594[63] = 0;
   out_6435812489740243594[64] = 0;
   out_6435812489740243594[65] = 0;
   out_6435812489740243594[66] = 0;
   out_6435812489740243594[67] = 0;
   out_6435812489740243594[68] = 0;
}
void h_31(double *state, double *unused, double *out_7114311135786219984) {
   out_7114311135786219984[0] = state[7];
   out_7114311135786219984[1] = state[8];
   out_7114311135786219984[2] = state[9];
}
void H_31(double *state, double *unused, double *out_5420128406978512594) {
   out_5420128406978512594[0] = 0;
   out_5420128406978512594[1] = 0;
   out_5420128406978512594[2] = 0;
   out_5420128406978512594[3] = 0;
   out_5420128406978512594[4] = 0;
   out_5420128406978512594[5] = 0;
   out_5420128406978512594[6] = 0;
   out_5420128406978512594[7] = 1;
   out_5420128406978512594[8] = 0;
   out_5420128406978512594[9] = 0;
   out_5420128406978512594[10] = 0;
   out_5420128406978512594[11] = 0;
   out_5420128406978512594[12] = 0;
   out_5420128406978512594[13] = 0;
   out_5420128406978512594[14] = 0;
   out_5420128406978512594[15] = 0;
   out_5420128406978512594[16] = 0;
   out_5420128406978512594[17] = 0;
   out_5420128406978512594[18] = 0;
   out_5420128406978512594[19] = 0;
   out_5420128406978512594[20] = 0;
   out_5420128406978512594[21] = 0;
   out_5420128406978512594[22] = 0;
   out_5420128406978512594[23] = 0;
   out_5420128406978512594[24] = 0;
   out_5420128406978512594[25] = 0;
   out_5420128406978512594[26] = 0;
   out_5420128406978512594[27] = 0;
   out_5420128406978512594[28] = 0;
   out_5420128406978512594[29] = 0;
   out_5420128406978512594[30] = 0;
   out_5420128406978512594[31] = 1;
   out_5420128406978512594[32] = 0;
   out_5420128406978512594[33] = 0;
   out_5420128406978512594[34] = 0;
   out_5420128406978512594[35] = 0;
   out_5420128406978512594[36] = 0;
   out_5420128406978512594[37] = 0;
   out_5420128406978512594[38] = 0;
   out_5420128406978512594[39] = 0;
   out_5420128406978512594[40] = 0;
   out_5420128406978512594[41] = 0;
   out_5420128406978512594[42] = 0;
   out_5420128406978512594[43] = 0;
   out_5420128406978512594[44] = 0;
   out_5420128406978512594[45] = 0;
   out_5420128406978512594[46] = 0;
   out_5420128406978512594[47] = 0;
   out_5420128406978512594[48] = 0;
   out_5420128406978512594[49] = 0;
   out_5420128406978512594[50] = 0;
   out_5420128406978512594[51] = 0;
   out_5420128406978512594[52] = 0;
   out_5420128406978512594[53] = 0;
   out_5420128406978512594[54] = 0;
   out_5420128406978512594[55] = 1;
   out_5420128406978512594[56] = 0;
   out_5420128406978512594[57] = 0;
   out_5420128406978512594[58] = 0;
   out_5420128406978512594[59] = 0;
   out_5420128406978512594[60] = 0;
   out_5420128406978512594[61] = 0;
   out_5420128406978512594[62] = 0;
   out_5420128406978512594[63] = 0;
   out_5420128406978512594[64] = 0;
   out_5420128406978512594[65] = 0;
   out_5420128406978512594[66] = 0;
   out_5420128406978512594[67] = 0;
   out_5420128406978512594[68] = 0;
}
void h_32(double *state, double *unused, double *out_6656960917418393346) {
   out_6656960917418393346[0] = state[3];
   out_6656960917418393346[1] = state[4];
   out_6656960917418393346[2] = state[5];
   out_6656960917418393346[3] = state[6];
}
void H_32(double *state, double *unused, double *out_5465957462164714200) {
   out_5465957462164714200[0] = 0;
   out_5465957462164714200[1] = 0;
   out_5465957462164714200[2] = 0;
   out_5465957462164714200[3] = 1;
   out_5465957462164714200[4] = 0;
   out_5465957462164714200[5] = 0;
   out_5465957462164714200[6] = 0;
   out_5465957462164714200[7] = 0;
   out_5465957462164714200[8] = 0;
   out_5465957462164714200[9] = 0;
   out_5465957462164714200[10] = 0;
   out_5465957462164714200[11] = 0;
   out_5465957462164714200[12] = 0;
   out_5465957462164714200[13] = 0;
   out_5465957462164714200[14] = 0;
   out_5465957462164714200[15] = 0;
   out_5465957462164714200[16] = 0;
   out_5465957462164714200[17] = 0;
   out_5465957462164714200[18] = 0;
   out_5465957462164714200[19] = 0;
   out_5465957462164714200[20] = 0;
   out_5465957462164714200[21] = 0;
   out_5465957462164714200[22] = 0;
   out_5465957462164714200[23] = 0;
   out_5465957462164714200[24] = 0;
   out_5465957462164714200[25] = 0;
   out_5465957462164714200[26] = 0;
   out_5465957462164714200[27] = 1;
   out_5465957462164714200[28] = 0;
   out_5465957462164714200[29] = 0;
   out_5465957462164714200[30] = 0;
   out_5465957462164714200[31] = 0;
   out_5465957462164714200[32] = 0;
   out_5465957462164714200[33] = 0;
   out_5465957462164714200[34] = 0;
   out_5465957462164714200[35] = 0;
   out_5465957462164714200[36] = 0;
   out_5465957462164714200[37] = 0;
   out_5465957462164714200[38] = 0;
   out_5465957462164714200[39] = 0;
   out_5465957462164714200[40] = 0;
   out_5465957462164714200[41] = 0;
   out_5465957462164714200[42] = 0;
   out_5465957462164714200[43] = 0;
   out_5465957462164714200[44] = 0;
   out_5465957462164714200[45] = 0;
   out_5465957462164714200[46] = 0;
   out_5465957462164714200[47] = 0;
   out_5465957462164714200[48] = 0;
   out_5465957462164714200[49] = 0;
   out_5465957462164714200[50] = 0;
   out_5465957462164714200[51] = 1;
   out_5465957462164714200[52] = 0;
   out_5465957462164714200[53] = 0;
   out_5465957462164714200[54] = 0;
   out_5465957462164714200[55] = 0;
   out_5465957462164714200[56] = 0;
   out_5465957462164714200[57] = 0;
   out_5465957462164714200[58] = 0;
   out_5465957462164714200[59] = 0;
   out_5465957462164714200[60] = 0;
   out_5465957462164714200[61] = 0;
   out_5465957462164714200[62] = 0;
   out_5465957462164714200[63] = 0;
   out_5465957462164714200[64] = 0;
   out_5465957462164714200[65] = 0;
   out_5465957462164714200[66] = 0;
   out_5465957462164714200[67] = 0;
   out_5465957462164714200[68] = 0;
   out_5465957462164714200[69] = 0;
   out_5465957462164714200[70] = 0;
   out_5465957462164714200[71] = 0;
   out_5465957462164714200[72] = 0;
   out_5465957462164714200[73] = 0;
   out_5465957462164714200[74] = 0;
   out_5465957462164714200[75] = 1;
   out_5465957462164714200[76] = 0;
   out_5465957462164714200[77] = 0;
   out_5465957462164714200[78] = 0;
   out_5465957462164714200[79] = 0;
   out_5465957462164714200[80] = 0;
   out_5465957462164714200[81] = 0;
   out_5465957462164714200[82] = 0;
   out_5465957462164714200[83] = 0;
   out_5465957462164714200[84] = 0;
   out_5465957462164714200[85] = 0;
   out_5465957462164714200[86] = 0;
   out_5465957462164714200[87] = 0;
   out_5465957462164714200[88] = 0;
   out_5465957462164714200[89] = 0;
   out_5465957462164714200[90] = 0;
   out_5465957462164714200[91] = 0;
}
void h_13(double *state, double *unused, double *out_7669147739270683552) {
   out_7669147739270683552[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7];
   out_7669147739270683552[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8];
   out_7669147739270683552[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9];
}
void H_13(double *state, double *unused, double *out_3028851125003744530) {
   out_3028851125003744530[0] = 0;
   out_3028851125003744530[1] = 0;
   out_3028851125003744530[2] = 0;
   out_3028851125003744530[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
   out_3028851125003744530[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
   out_3028851125003744530[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7];
   out_3028851125003744530[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
   out_3028851125003744530[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2);
   out_3028851125003744530[8] = 2*state[3]*state[6] + 2*state[4]*state[5];
   out_3028851125003744530[9] = -2*state[3]*state[5] + 2*state[4]*state[6];
   out_3028851125003744530[10] = 0;
   out_3028851125003744530[11] = 0;
   out_3028851125003744530[12] = 0;
   out_3028851125003744530[13] = 0;
   out_3028851125003744530[14] = 0;
   out_3028851125003744530[15] = 0;
   out_3028851125003744530[16] = 0;
   out_3028851125003744530[17] = 0;
   out_3028851125003744530[18] = 0;
   out_3028851125003744530[19] = 0;
   out_3028851125003744530[20] = 0;
   out_3028851125003744530[21] = 0;
   out_3028851125003744530[22] = 0;
   out_3028851125003744530[23] = 0;
   out_3028851125003744530[24] = 0;
   out_3028851125003744530[25] = 0;
   out_3028851125003744530[26] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7];
   out_3028851125003744530[27] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
   out_3028851125003744530[28] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
   out_3028851125003744530[29] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8];
   out_3028851125003744530[30] = -2*state[3]*state[6] + 2*state[4]*state[5];
   out_3028851125003744530[31] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2);
   out_3028851125003744530[32] = 2*state[3]*state[4] + 2*state[5]*state[6];
   out_3028851125003744530[33] = 0;
   out_3028851125003744530[34] = 0;
   out_3028851125003744530[35] = 0;
   out_3028851125003744530[36] = 0;
   out_3028851125003744530[37] = 0;
   out_3028851125003744530[38] = 0;
   out_3028851125003744530[39] = 0;
   out_3028851125003744530[40] = 0;
   out_3028851125003744530[41] = 0;
   out_3028851125003744530[42] = 0;
   out_3028851125003744530[43] = 0;
   out_3028851125003744530[44] = 0;
   out_3028851125003744530[45] = 0;
   out_3028851125003744530[46] = 0;
   out_3028851125003744530[47] = 0;
   out_3028851125003744530[48] = 0;
   out_3028851125003744530[49] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7];
   out_3028851125003744530[50] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7];
   out_3028851125003744530[51] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8];
   out_3028851125003744530[52] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9];
   out_3028851125003744530[53] = 2*state[3]*state[5] + 2*state[4]*state[6];
   out_3028851125003744530[54] = -2*state[3]*state[4] + 2*state[5]*state[6];
   out_3028851125003744530[55] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2);
   out_3028851125003744530[56] = 0;
   out_3028851125003744530[57] = 0;
   out_3028851125003744530[58] = 0;
   out_3028851125003744530[59] = 0;
   out_3028851125003744530[60] = 0;
   out_3028851125003744530[61] = 0;
   out_3028851125003744530[62] = 0;
   out_3028851125003744530[63] = 0;
   out_3028851125003744530[64] = 0;
   out_3028851125003744530[65] = 0;
   out_3028851125003744530[66] = 0;
   out_3028851125003744530[67] = 0;
   out_3028851125003744530[68] = 0;
}
void h_14(double *state, double *unused, double *out_6849354457489368967) {
   out_6849354457489368967[0] = state[10];
   out_6849354457489368967[1] = state[11];
   out_6849354457489368967[2] = state[12];
}
void H_14(double *state, double *unused, double *out_7925197860050783002) {
   out_7925197860050783002[0] = 0;
   out_7925197860050783002[1] = 0;
   out_7925197860050783002[2] = 0;
   out_7925197860050783002[3] = 0;
   out_7925197860050783002[4] = 0;
   out_7925197860050783002[5] = 0;
   out_7925197860050783002[6] = 0;
   out_7925197860050783002[7] = 0;
   out_7925197860050783002[8] = 0;
   out_7925197860050783002[9] = 0;
   out_7925197860050783002[10] = 1;
   out_7925197860050783002[11] = 0;
   out_7925197860050783002[12] = 0;
   out_7925197860050783002[13] = 0;
   out_7925197860050783002[14] = 0;
   out_7925197860050783002[15] = 0;
   out_7925197860050783002[16] = 0;
   out_7925197860050783002[17] = 0;
   out_7925197860050783002[18] = 0;
   out_7925197860050783002[19] = 0;
   out_7925197860050783002[20] = 0;
   out_7925197860050783002[21] = 0;
   out_7925197860050783002[22] = 0;
   out_7925197860050783002[23] = 0;
   out_7925197860050783002[24] = 0;
   out_7925197860050783002[25] = 0;
   out_7925197860050783002[26] = 0;
   out_7925197860050783002[27] = 0;
   out_7925197860050783002[28] = 0;
   out_7925197860050783002[29] = 0;
   out_7925197860050783002[30] = 0;
   out_7925197860050783002[31] = 0;
   out_7925197860050783002[32] = 0;
   out_7925197860050783002[33] = 0;
   out_7925197860050783002[34] = 1;
   out_7925197860050783002[35] = 0;
   out_7925197860050783002[36] = 0;
   out_7925197860050783002[37] = 0;
   out_7925197860050783002[38] = 0;
   out_7925197860050783002[39] = 0;
   out_7925197860050783002[40] = 0;
   out_7925197860050783002[41] = 0;
   out_7925197860050783002[42] = 0;
   out_7925197860050783002[43] = 0;
   out_7925197860050783002[44] = 0;
   out_7925197860050783002[45] = 0;
   out_7925197860050783002[46] = 0;
   out_7925197860050783002[47] = 0;
   out_7925197860050783002[48] = 0;
   out_7925197860050783002[49] = 0;
   out_7925197860050783002[50] = 0;
   out_7925197860050783002[51] = 0;
   out_7925197860050783002[52] = 0;
   out_7925197860050783002[53] = 0;
   out_7925197860050783002[54] = 0;
   out_7925197860050783002[55] = 0;
   out_7925197860050783002[56] = 0;
   out_7925197860050783002[57] = 0;
   out_7925197860050783002[58] = 1;
   out_7925197860050783002[59] = 0;
   out_7925197860050783002[60] = 0;
   out_7925197860050783002[61] = 0;
   out_7925197860050783002[62] = 0;
   out_7925197860050783002[63] = 0;
   out_7925197860050783002[64] = 0;
   out_7925197860050783002[65] = 0;
   out_7925197860050783002[66] = 0;
   out_7925197860050783002[67] = 0;
   out_7925197860050783002[68] = 0;
}
void h_19(double *state, double *unused, double *out_2880896277160693049) {
   out_2880896277160693049[0] = state[20];
   out_2880896277160693049[1] = state[21];
   out_2880896277160693049[2] = state[22];
}
void H_19(double *state, double *unused, double *out_623983078872615106) {
   out_623983078872615106[0] = 0;
   out_623983078872615106[1] = 0;
   out_623983078872615106[2] = 0;
   out_623983078872615106[3] = 0;
   out_623983078872615106[4] = 0;
   out_623983078872615106[5] = 0;
   out_623983078872615106[6] = 0;
   out_623983078872615106[7] = 0;
   out_623983078872615106[8] = 0;
   out_623983078872615106[9] = 0;
   out_623983078872615106[10] = 0;
   out_623983078872615106[11] = 0;
   out_623983078872615106[12] = 0;
   out_623983078872615106[13] = 0;
   out_623983078872615106[14] = 0;
   out_623983078872615106[15] = 0;
   out_623983078872615106[16] = 0;
   out_623983078872615106[17] = 0;
   out_623983078872615106[18] = 0;
   out_623983078872615106[19] = 0;
   out_623983078872615106[20] = 1;
   out_623983078872615106[21] = 0;
   out_623983078872615106[22] = 0;
   out_623983078872615106[23] = 0;
   out_623983078872615106[24] = 0;
   out_623983078872615106[25] = 0;
   out_623983078872615106[26] = 0;
   out_623983078872615106[27] = 0;
   out_623983078872615106[28] = 0;
   out_623983078872615106[29] = 0;
   out_623983078872615106[30] = 0;
   out_623983078872615106[31] = 0;
   out_623983078872615106[32] = 0;
   out_623983078872615106[33] = 0;
   out_623983078872615106[34] = 0;
   out_623983078872615106[35] = 0;
   out_623983078872615106[36] = 0;
   out_623983078872615106[37] = 0;
   out_623983078872615106[38] = 0;
   out_623983078872615106[39] = 0;
   out_623983078872615106[40] = 0;
   out_623983078872615106[41] = 0;
   out_623983078872615106[42] = 0;
   out_623983078872615106[43] = 0;
   out_623983078872615106[44] = 1;
   out_623983078872615106[45] = 0;
   out_623983078872615106[46] = 0;
   out_623983078872615106[47] = 0;
   out_623983078872615106[48] = 0;
   out_623983078872615106[49] = 0;
   out_623983078872615106[50] = 0;
   out_623983078872615106[51] = 0;
   out_623983078872615106[52] = 0;
   out_623983078872615106[53] = 0;
   out_623983078872615106[54] = 0;
   out_623983078872615106[55] = 0;
   out_623983078872615106[56] = 0;
   out_623983078872615106[57] = 0;
   out_623983078872615106[58] = 0;
   out_623983078872615106[59] = 0;
   out_623983078872615106[60] = 0;
   out_623983078872615106[61] = 0;
   out_623983078872615106[62] = 0;
   out_623983078872615106[63] = 0;
   out_623983078872615106[64] = 0;
   out_623983078872615106[65] = 0;
   out_623983078872615106[66] = 0;
   out_623983078872615106[67] = 0;
   out_623983078872615106[68] = 1;
}
}

extern "C"{
#define DIM 23
#define EDIM 22
#define MEDIM 22
typedef void (*Hfun)(double *, double *, double *);

void predict(double *x, double *P, double *Q, double dt);
const static double MAHA_THRESH_3 = 3.841459;
void update_3(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_4 = 7.814728;
void update_4(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_9 = 7.814728;
void update_9(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_10 = 7.814728;
void update_10(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_12 = 7.814728;
void update_12(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_31 = 7.814728;
void update_31(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_32 = 9.487729;
void update_32(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_13 = 7.814728;
void update_13(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_14 = 7.814728;
void update_14(double *, double *, double *, double *, double *);
const static double MAHA_THRESH_19 = 7.814728;
void update_19(double *, double *, double *, double *, double *);
}

#include <eigen3/Eigen/Dense>
#include <iostream>

typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;

void predict(double *in_x, double *in_P, double *in_Q, double dt) {
  typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;

  double nx[DIM] = {0};
  double in_F[EDIM*EDIM] = {0};

  // functions from sympy
  f_fun(in_x, dt, nx);
  F_fun(in_x, dt, in_F);


  EEM F(in_F);
  EEM P(in_P);
  EEM Q(in_Q);

  RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
  P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
  P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
  P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();

  P = P + dt*Q;

  // copy out state
  memcpy(in_x, nx, DIM * sizeof(double));
  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
}

// note: extra_args dim only correct when null space projecting
// otherwise 1
template <int ZDIM, int EADIM, bool MAHA_TEST>
void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
  typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
  typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
  typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
  //typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;

  double in_hx[ZDIM] = {0};
  double in_H[ZDIM * DIM] = {0};
  double in_H_mod[EDIM * DIM] = {0};
  double delta_x[EDIM] = {0};
  double x_new[DIM] = {0};


  // state x, P
  Eigen::Matrix<double, ZDIM, 1> z(in_z);
  EEM P(in_P);
  ZZM pre_R(in_R);

  // functions from sympy
  h_fun(in_x, in_ea, in_hx);
  H_fun(in_x, in_ea, in_H);
  ZDM pre_H(in_H);

  // get y (y = z - hx)
  Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
  X1M y; XXM H; XXM R;
  if (Hea_fun){
    typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
    double in_Hea[ZDIM * EADIM] = {0};
    Hea_fun(in_x, in_ea, in_Hea);
    ZAM Hea(in_Hea);
    XXM A = Hea.transpose().fullPivLu().kernel();


    y = A.transpose() * pre_y;
    H = A.transpose() * pre_H;
    R = A.transpose() * pre_R * A;
  } else {
    y = pre_y;
    H = pre_H;
    R = pre_R;
  }
  // get modified H
  H_mod_fun(in_x, in_H_mod);
  DEM H_mod(in_H_mod);
  XEM H_err = H * H_mod;

  // Do mahalobis distance test
  if (MAHA_TEST){
    XXM a = (H_err * P * H_err.transpose() + R).inverse();
    double maha_dist = y.transpose() * a * y;
    if (maha_dist > MAHA_THRESHOLD){
      R = 1.0e16 * R;
    }
  }

  // Outlier resilient weighting
  double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());

  // kalman gains and I_KH
  XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
  XEM KT = S.fullPivLu().solve(H_err * P.transpose());
  //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
  //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
  //std::cout << "Here is the matrix rot:\n" << K << std::endl;
  EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);

  // update state by injecting dx
  Eigen::Matrix<double, EDIM, 1> dx(delta_x);
  dx  = (KT.transpose() * y);
  memcpy(delta_x, dx.data(), EDIM * sizeof(double));
  err_fun(in_x, delta_x, x_new);
  Eigen::Matrix<double, DIM, 1> x(x_new);

  // update cov
  P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);

  // copy out state
  memcpy(in_x, x.data(), DIM * sizeof(double));
  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
  memcpy(in_z, y.data(), y.rows() * sizeof(double));
}



extern "C"{

      void update_3(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<1,3,0>(in_x, in_P, h_3, H_3, NULL, in_z, in_R, in_ea, MAHA_THRESH_3);
      }
    
      void update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_4, H_4, NULL, in_z, in_R, in_ea, MAHA_THRESH_4);
      }
    
      void update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_9, H_9, NULL, in_z, in_R, in_ea, MAHA_THRESH_9);
      }
    
      void update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_10, H_10, NULL, in_z, in_R, in_ea, MAHA_THRESH_10);
      }
    
      void update_12(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_12, H_12, NULL, in_z, in_R, in_ea, MAHA_THRESH_12);
      }
    
      void update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
      }
    
      void update_32(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<4,3,0>(in_x, in_P, h_32, H_32, NULL, in_z, in_R, in_ea, MAHA_THRESH_32);
      }
    
      void update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_13, H_13, NULL, in_z, in_R, in_ea, MAHA_THRESH_13);
      }
    
      void update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14);
      }
    
      void update_19(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
        update<3,3,0>(in_x, in_P, h_19, H_19, NULL, in_z, in_R, in_ea, MAHA_THRESH_19);
      }
    
}
